import rosbag
import csv
import math
from sensor_msgs.msg import NavSatFix
from geometry_msgs.msg import Quaternion
from tf.transformations import quaternion_from_euler

# 输入ROS bag文件路径
bag_file = '/media/atakanoinu/MSDAD/[6]corridor1/[1]corridor1_midday/corridor1_midday.bag'

# 输出CSV文件路径
csv_file = '/media/atakanoinu/MSDAD/file_backup/output.csv'

# 定义CSV文件的表头
csv_header = ['Timestamp', 'Latitude', 'Longitude', 'Altitude', 'Quaternion_W', 'Quaternion_X', 'Quaternion_Y', 'Quaternion_Z']

currenttimestamp = 1711594638.05

# 读取ROS bag中的定位信息并写入CSV文件
with rosbag.Bag(bag_file, 'r') as bag:
    with open(csv_file, 'w') as file:
        writer = csv.writer(file)
        writer.writerow(csv_header)

        for topic, msg, t in bag.read_messages(topics=['/ins/localization']):
            # 获取定位信息
            timestamp = currenttimestamp 
            currenttimestamp += 0.01
            
            latitude = msg.latitude
            longitude = msg.longitude
            altitude = msg.altitude

            # 将欧拉角转换为四元数
            roll = msg.roll
            pitch = msg.pitch
            yaw = msg.azimuth
            quaternion = quaternion_from_euler(roll, pitch, yaw)

            # 写入CSV文件
            writer.writerow([timestamp, '1', latitude, longitude, altitude, quaternion[3], quaternion[0], quaternion[1], quaternion[2]])